/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************
 *  @file x.h
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#ifndef VEHICLEPOSITIONMANAGE_H
#define VEHICLEPOSITIONMANAGE_H
#include "GeneralDefine.h"
#include <QMutex>

class FilteringAlgorithm;
class MotionMessage;

/**
 * @brief The VehiclePositionManage class agv位置管理类
 */
class VehiclePositionManage
{
public:
    VehiclePositionManage(int robotIdIn,MotionMessage * motionMessageIn,QString odomFileIn,
                          QString positionConfigFileIn,double deltaTimeIn);

public:
    bool isReflectNeedInitialPosition();
    int getVehiclePositionCalibrateInfo(struct VehiclePosition &infoOut);
    int calibrateVehicle(double xOffset,double yOffset,double rOffset);

    int setCheckVehiclePosition(E_VEHICLE_POSITION_TYPE typeIn,bool isCheck);
    int setCheckVehiclePosition_vmark(bool isCheck);
    int setCheckVehiclePosition_amcl(bool isCheck);
    int setCheckVehiclePosition_qrcode(bool isCheck);
    int getVehicleSpeedFeedback(VehicleVelocity &velocityOut);
    int getOdometer(double &historyOdomOut,double &historyTimeOut,double &customOdomOut,double &customeTimeOut);
    int resetCustomOdometer();
    int saveOdometerToFile();
    int reset();

    int updateVehicleVelocity(VehicleVelocity velocityIn,struct timespec velocityUpdataStampIn);
    int changeToPositionType(E_VEHICLE_POSITION_TYPE typeIn);
    int getVehiclePositionType(E_VEHICLE_POSITION_TYPE &typeOut);
    /**
     * @brief getVehiclePosition
     * @param positionReturn
     * @param typeOut 当前默认的位置类型
     * @return
     */
    int getVehiclePosition_default(VehiclePosition& positionReturn,E_VEHICLE_POSITION_TYPE &typeOut);
    /**
     * @brief getVehiclePosition
     * @param positionReturn
     * @param typeOut　当前默认的位置类型
     * @return
     */
    int getVehiclePosition_default(PositionInfo& positionReturn,E_VEHICLE_POSITION_TYPE &typeOut);
    /**
     * @brief getVehiclePosition
     * @param positionReturn
     * @param typeIn 用户要求的位置类型
     * @return
     */
    int getVehiclePosition_specified(PositionInfo& positionReturn,E_VEHICLE_POSITION_TYPE typeIn);

    int updateVehiclePosition(PositionInfo positionInfoIn,E_VEHICLE_POSITION_TYPE typeIn);
    int initialVehiclePosition(E_VEHICLE_POSITION_TYPE typeIn);
    int setReflectPositionCertainity(VehiclePosition uncertainPoseIn,E_VEHICLE_POSITION_TYPE typeIn);//算法用于暂停和报错。
    int getReflectPositionStatus(bool &isOk);


private:

    /**
     * @brief angleRotationJudgement 考虑哪种角度旋转方法最近
     * @param targetAngle
     * @param initialAngle
     * @return
     */
    double angleRotationJudgement(double targetAngle,double initialAngle);//到目标姿态的最小路径转角

    int setCurrentVehiclePosition(VehiclePosition positionIn);

    int saveVehicleOffsetToFile();
    int checkPosition_pcl(PositionInfo positionInfo);
    int checkPosition_amcl(PositionInfo positionInfo);
    int checkPosition_vmark(PositionInfo positionInfo);
    int checkPosition_qrcode(PositionInfo positionInfo);
    int checkPosition_cu1(PositionInfo positionInfo);
    int calculateOdometerLoop();
    int initialOdomFile(QString odomFileIn);
    int initialPositionConfigFile(QString positionConfigFileIn);
    /**
     * @brief checkPositionFeedbackIsResonable 跳变大于最大速度不合理，跳变加速度大于最大加速度不合理
     * @param positionInfoIn
     * @return
     */
    int checkPositionFeedbackIsResonable(E_VEHICLE_POSITION_TYPE typeIn);
    int checkPositionFeedbackIsResonable_specified(PositionInfo posNew,PositionInfo posOld, PositionInfo posOOld);
    int calculatePreditionPosition();
    void addMsg(int messageLevel, std::string componentName, std::string messageType
                , int messageCode, int robotIdIn, int parameter1=0, int parameter2=0, int parameter3=0, int parameter4=0);
private:
    PositionInfo vehiclePositionFeedback_ros;
    PositionInfo vehiclePositionFeedback_ros_old;
    PositionInfo vehiclePositionFeedback_ros_oold;
    VehiclePosition positionBias_ros;
    PositionInfo vehiclePositionFeedback_encoder_odom;
    VehiclePosition positionBias_encoder_odom;
    PositionInfo vehiclePositionFeedback_imu_odom;
    VehiclePosition positionBias_imu_odom;

    PositionInfo vehiclePositionFeedback_vmark;
    PositionInfo vehiclePositionFeedback_vmark_old;
    PositionInfo vehiclePositionFeedback_vmark_oold;
    VehiclePosition positionBias_vmark;
    volatile bool vmarkIsUpdateSucess;

    PositionInfo vehiclePositionFeedback_qrcode;
    PositionInfo vehiclePositionFeedback_qrcode_old;
    PositionInfo vehiclePositionFeedback_qrcode_oold;
    VehiclePosition positionBias_qrcode;
    volatile bool qrcodeIsUpdateSucess;


    PositionInfo vehiclePositionFeedback_cu1;
    PositionInfo vehiclePositionFeedback_cu1_old;
    PositionInfo vehiclePositionFeedback_cu1_oold;
    VehiclePosition positionBias_cu1;
    volatile bool cu1IsUpdateSucess;

    PositionInfo vehiclePositionFeedback_pcl;
    VehiclePosition positionBias_pcl;
    PositionInfo vehiclePositionFeedback_UWB;
    VehiclePosition positionBias_UWB;
    PositionInfo vehiclePositionFeedback_prediction;
    VehiclePosition positionBias_prediction;

    VehicleVelocity currentVehicleVelocity;
    struct timespec velocityUpdataStamp;
    struct timespec velocityUpdataStamp_old;

    E_VEHICLE_POSITION_TYPE currentPositionType;
//    E_VEHICLE_POSITION_TYPE favoritePositionType;

    //需要根据里程计速度反馈进行位置滤波，才能有较好的平滑效果，又不影响跟踪性能．
    FilteringAlgorithm* filtering_rosPosition_x;//not used
    FilteringAlgorithm* filtering_rosPosition_y;
    FilteringAlgorithm* filtering_rosPosition_r;
    int initialPositionNoFilterCount_ros;

    FilteringAlgorithm* filtering_vmarkPosition_x;//not used
    FilteringAlgorithm* filtering_vmarkPosition_y;
    FilteringAlgorithm* filtering_vmarkPosition_r;
    int initialPositionNoFilterCount_vmark;

    FilteringAlgorithm* filtering_reflectPosition_x;//not used
    FilteringAlgorithm* filtering_reflectPosition_y;
    FilteringAlgorithm* filtering_reflectPosition_r;
    int initialPositionNoFilterCount_reflect;

    double deltaTime;
    int robotId;
    double maxVel_line;//m/s
    double maxAcc_line;//m/s^2
    double maxVel_rotate;//rad/s
    double maxAcc_rotate;//rad/s^2

    MotionMessage * motionMessage;
    double historyOdometer;//m
    double historyTime;//S
    double customOdometer;//m
    double customTime;//S

    double maxMumPclMatchScore;
    double minimumAmclMatchScore;//amcl最低匹配分值

    QString odomFile;
    QString positionConfigFile;

    double vehicle_x_offset;
    double vehicle_y_offset;
    double vehicle_r_offset;

    int amcl_allow_max_error_count;
    int vmark_allow_max_error_count;
    int qrcode_allow_max_error_count;
    bool isCheckVehiclePosition_vmark;
    bool isCheckVehiclePosition_qrcode;
    bool isCheckVehiclePosition_amcl;
    bool isCheckVehiclePosition_cu1;

    double reflect_allow_distance_tolorence;//允许距离误差 m，超过后提示并暂停运动。
    double reflect_allow_angle_tolorence;//允许角度误差 rad，超过后提示并暂停运动。读取ｘml文件时转换了单位
    int reflect_allow_max_error_count;
    VehiclePosition reflect_uncertainPose;
    VehiclePosition vmark_uncertainPose;
    bool isReflectPositionOk;//定位累计误差超过允许值则不ok
    bool isVmarkPositionOk;
    int reflect_allow_position_change_count;
    QMutex positionUpdateMutex;
    double vmark_allow_angle_tolorence;
    double vmark_allow_distance_tolorence;

    double ros_position_filter_line_ratio;
    double ros_position_filter_angle_ratio;
    double reflect_position_filter_line_ratio;
    double reflect_position_filter_angle_ratio;
    double reflect_allowMaxDistanceOffsetBewteenAmcl;
    double reflect_allowMaxAngleOffsetBewteenAmcl;//读配置文件后，转换为rad

    double vmark_position_filter_line_ratio;
    double vmark_position_filter_angle_ratio;

};

#endif // VEHICLEPOSITIONMANAGE_H
